#include <slasyf_rk.h>

void SLASYF_RK(char uplo, int n, int nb, int kb, float *a,
               int lda, float *e, int *ipiv, float *w, int ldw, int info)
{
    int k, kw, kStep, p, iMax, jMax,
        iTemp, kk, kp, kkw, jb;
    int j, ii, jj, flag;
    float sfMin, absAKK, colMax, rowMax, sTemp, r1,
        d11, d12, d21, d22, t;
    info = 0;
    const float ALPHA = (ONE + sqrt(SEVTEN)) / EIGHT;
    char sla[] = {'S'};
    // Compute machine safe minimum
    sfMin = Slamch(sla);
    if (KmlLsame(uplo, 'U'))
    {
        e[0] = ZERO;
        k = n;
        while (1)
        {
            kw = nb + k - n; // kw is the column of w which corresponds to column k of a
            if ((k <= n - nb + 1 && nb < n) || k < 1)
            {
                break;
            }
            kStep = 1;
            p = k;
            // Copy column k of a to column kw of w and update it
            cblas_scopy(k, a + (k - 1) * lda, 1, w + (kw - 1) * lda, 1);
            if (k < n)
            {
                cblas_sgemv(CblasColMajor, CblasNoTrans, k, n - k, -ONE, a + k * lda,
                            lda, w + k - 1 + kw * ldw, ldw, ONE, w + (kw - 1) * ldw, 1);
            }
            /*
            Determine rows and columns to be interchanged and
            whether*a 1 - by - 1 or 2 - by - 2 pivot block will be used
            */
            absAKK = fabs(w[k - 1 + (kw - 1) * ldw]);
            /*
            IMAX is the row-index of the largest off-diagonal element in
            column k, and COLMAX is its absolute value.
            Determine both COLMAX and IMAX.
            */
            if (k > 1)
            {
                iMax = cblas_isamax(k - 1, w + (kw - 1) * ldw, 1) + 1;
                colMax = fabs(w[iMax - 1 + (kw - 1) * ldw]);
            }
            else
            {
                colMax = ZERO;
            }
            if (MAX(absAKK, colMax) == ZERO)
            {
                // Column k is zero or underflow: set info and continue
                if (info == 0)
                {
                    info = k;
                }
                kp = k; // TODO:what is kp?
                cblas_scopy(k, w + (kw - 1) * ldw, 1, a + (k - 1) * lda, 1);
                // Set e( k ) to zero
                if (k > 1)
                {
                    e[k - 1] = ZERO;
                }
            }
            else
            {
                // Test for interchange

                /*
                Equivalent to testing for ABSAKK.GE.ALPHA*COLMAX
                (used to handle NaN and Inf)
                */
                if (absAKK >= ALPHA * colMax)
                {
                    // no interchange, use 1-by-1 pivot block
                    kp = k;
                }
                else
                {
                    flag = 1;
                    // Loop until pivot found
                    while (flag)
                    {
                        // Copy column IMAX to column KW-1 of w and update it
                        cblas_scopy(iMax, a + (iMax - 1) * lda, 1, w + (kw - 2) * ldw, 1);
                        cblas_scopy(k - iMax, a + iMax - 1 + iMax * lda, lda, w + iMax + (kw - 2) * ldw, 1);
                        if (k < n)
                        {
                            cblas_sgemv(CblasColMajor, CblasNoTrans, k, n - k, -ONE, a + k * lda,
                                        lda, w + iMax - 1 + kw * ldw, ldw, ONE, w + (kw - 2) * ldw, 1);
                        }
                        /*
                        JMAX is the column-index of the largest off-diagonal
                        element in row IMAX, and ROWMAX is its absolute value.
                        Determine both ROWMAX and JMAX.
                        */
                        if (iMax != k)
                        {
                            jMax = iMax + cblas_isamax(k - iMax, w + iMax + (kw - 2) * ldw, 1) + 1;
                            rowMax = fabs(w[jMax - 1 + (kw - 2) * ldw]);
                        }
                        else
                        {
                            rowMax = ZERO;
                        }

                        if (iMax > 1)
                        {
                            iTemp = cblas_isamax(iMax - 1, w + (kw - 2) * ldw, 1) + 1;
                            sTemp = fabs(w[iTemp - 1 + (kw - 2) * ldw]);
                            if (sTemp > rowMax)
                            {
                                rowMax = sTemp;
                                jMax = iTemp;
                            }
                        }

                        /*
                        Equivalent to testing for ABS( w( IMAX, KW-1 ) ).GE.ALPHA*ROWMAX
                        (used to handle NaN and Inf)
                        */
                        if (fabs(w[iMax - 1 + (kw - 2) * ldw]) >= ALPHA * rowMax)
                        {
                            // interchange rows and columns K and IMAX,use 1-by-1 pivot block
                            kp = iMax;
                            // copy column KW-1 of w to column KW of w
                            cblas_scopy(k, w + (kw - 2) * ldw, 1, w + (kw - 1) * ldw, 1);
                            flag = 0;
                        }
                        else if ((p == jMax) || (rowMax <= colMax)) // Equivalent to testing for ROWMAX.EQ.COLMAX,(used to handle NaN and Inf)
                        {
                            // interchange rows and columns K-1 and IMAX,use 2-by-2 pivot block
                            kp = iMax;
                            kStep = 2;
                            flag = 0;
                        }
                        else
                        {
                            // Pivot not found: set params and repeat
                            p = iMax;
                            colMax = rowMax;
                            iMax = jMax;
                            // Copy updated JMAXth (next IMAXth) column to Kth of w
                            cblas_scopy(k, w + (kw - 2) * ldw, 1, w + (kw - 1) * ldw, 1);
                        }
                    }
                }

                kk = k - kStep + 1;
                // KKW is the column of w which corresponds to column KK of a
                kkw = nb + kk - n;
                if ((kStep == 2) && (p != k))
                {
                    // Copy non-updated column K to column P
                    cblas_scopy(k - p, a + p + (k - 1) * lda, 1, a + p - 1 + p * lda, lda);
                    cblas_scopy(p, a + (k - 1) * lda, 1, a + (p - 1) * lda, 1);
                    // Interchange rows K and P in last N - K + 1 columns of a and last N-K+2 columns of w
                    cblas_sswap(n - k + 1, a + k - 1 + (k - 1) * lda, lda, a + p - 1 + (k - 1) * lda, lda);
                    cblas_sswap(n - kk + 1, w + k - 1 + (kkw - 1) * ldw, ldw, w + p - 1 + (kkw - 1) * ldw, ldw);
                }

                // Updated column kp is already stored in column KKW of w
                if (kp != kk)
                {
                    // Copy non-updated column KK to column kp
                    a[kp - 1 + (k - 1) * lda] = a[kk - 1 + (k - 1) * lda];
                    cblas_scopy(k - 1 - kp, a + kp + (kk - 1) * lda, 1, a + kp - 1 + kp * lda, lda);
                    cblas_scopy(kp, a + (kk - 1) * lda, 1, a + (kp - 1) * lda, 1);
                    // Interchange rows KK and kp in last N-KK+1 columns
                    // of a and w
                    cblas_sswap(n - kk + 1, a + kk - 1 + (kk - 1) * lda, lda, a + kp - 1 + (kk - 1) * lda, lda);
                    cblas_sswap(n - kk + 1, w + kk - 1 + (kkw - 1) * ldw, ldw, w + kp - 1 + (kkw - 1) * ldw, ldw);
                }

                if (kStep == 1)
                {
                    /*
                    1-by-1 pivot block D(k): column KW of w now holds
                    w(k) = U(k)*D(k),where U(k) is the k-th column of U
                    */
                    // Store U(k) in column k of a
                    cblas_scopy(k, w + (kw - 1) * ldw, 1, a + (k - 1) * lda, 1);
                    if (k > 1)
                    {
                        if (fabs(a[k - 1 + (k - 1) * lda]) >= sfMin)
                        {
                            r1 = ONE / a[k - 1 + (k - 1) * lda];
                            cblas_sscal(k - 1, r1, a + (k - 1) * lda, 1);
                        }
                        else if (a[k - 1 + (k - 1) * lda] != ZERO)
                        {
                            for (ii = 1; ii <= k - 1; ii++)
                            {
                                a[ii - 1 + (k - 1) * lda] = a[ii - 1 + (k - 1) * lda] / a[k - 1 + (k - 1) * lda];
                            }
                        }
                        // Store the superdiagonal element of D in array E
                        e[k - 1] = ZERO;
                    }
                }
                else
                {
                    /*
                    2-by-2 pivot block D(k): columns KW and KW-1 of w now hold
                    ( w(k-1) w(k) ) = ( U(k-1) U(k) )*D(k)
                    where U(k) and U(k-1) are the k-th and (k-1)-th columnsof U
                    */
                    if (k > 2)
                    {
                        // Store U(k) and U(k-1) in columns k and k-1 of a
                        d12 = w[k - 2 + (kw - 1) * ldw];
                        d11 = w[k - 1 + (kw - 1) * ldw] / d12;
                        d22 = w[k - 2 + (kw - 2) * ldw] / d12;
                        t = ONE / (d11 * d22 - ONE);
                        for (int j = 1; j <= k - 2; j++)
                        {
                            a[j - 1 + (k - 2) * lda] = t * ((d11 * w[j - 1 + (kw - 2) * ldw] -
                                                             w[j - 1 + (kw - 1) * ldw]) /
                                                            d12);
                            a[j - 1 + (k - 1) * lda] = t * ((d22 * w[j - 1 + (kw - 1) * ldw] -
                                                             w[j - 1 + (kw - 2) * ldw]) /
                                                            d12);
                        }
                    }
                    /*
                    Copy diagonal elements of D(K) to a,
                    copy superdiagonal element of D(K) to E(K) and
                    ZERO out superdiagonal entry of a
                    */
                    a[k - 2 + (k - 2) * lda] = w[k - 2 + (kw - 2) * ldw];
                    a[k - 2 + (k - 1) * lda] = ZERO;
                    a[k - 1 + (k - 1) * lda] = w[k - 1 + (kw - 1) * ldw];
                    e[k - 1] = w[k - 2 + (kw - 1) * ldw];
                    e[k - 2] = ZERO;
                }
            }
            // Store details of the interchanges in IPIV
            if (kStep == 1)
            {
                ipiv[k - 1] = kp;
            }
            else
            {
                ipiv[k - 1] = -p;
                ipiv[k - 2] = -kp;
            }
            k -= kStep;
        }
        /*
        Update the upper triangle of A11 (= a(1:k,1:k)) as
        A11 := A11 - U12*D*U12**T = A11 - U12*w**T
        computing blocks of NB columns at a time
        */
        for (j = ((k - 1) / nb) * nb + 1; j >= 1; j -= nb)
        {
            jb = MIN(nb, k - j + 1);
            // Update the upper triangle of the diagonal block
            for (jj = j; jj <= j + jb - 1; jj++)
            {
                cblas_sgemv(CblasColMajor, CblasNoTrans, jj - j + 1, n - k, -ONE, a + j - 1 + k * lda,
                            lda, w + jj - 1 + kw * ldw, ldw, ONE, a + j - 1 + (jj - 1) * lda, 1);
            }
            // Update the rectangular superdiagonal block
            if (j >= 2)
            {
                cblas_sgemm(CblasColMajor, CblasNoTrans, CblasTrans, j - 1, jb, n - k, -ONE, a + k * lda,
                            lda, w + j - 1 + kw * ldw, ldw, ONE, a + (j - 1) * lda, lda);
            }
        }
        // Set KB to the number of columns factorized
        kb = n - k;
    }
    else
    {
        /*
        Factorize the leading columns of a using the lower triangle of a
        and working forwards, and compute the matrix w = L21*D for use in updating A22
        */

        // Initialize the unused last entry of the subdiagonal array E.
        e[n - 1] = ZERO;
        // K is the main loop index, increasing from 1 in steps of 1 or 2
        k = 1;
        while (1)
        {
            if ((k >= nb && nb < n) || k > n)
            {
                break;
            }
            kStep = 1;
            p = k;
            // Copy column K of a to column K of w and update it
            cblas_scopy(n - k + 1, a + k - 1 + (k - 1) * lda, 1, w + k - 1 + (k - 1) * ldw, 1);
            if (k > 1)
            {
                cblas_sgemv(CblasColMajor, CblasNoTrans, n - k + 1, k - 1, -ONE, a + k - 1,
                            lda, w + k - 1, ldw, ONE, w + k - 1 + (k - 1) * ldw, 1);
            }
            /*
            Determine rows and columns to be interchanged and whether
            a 1-by-1 or 2-by-2 pivot block will be used
            */
            absAKK = fabs(w[k - 1 + (k - 1) * ldw]);

            /*
            IMAX is the row-index of the largest off-diagonal element in
            column K, and COLMAX is its absolute value.
            */

            // Determine both COLMAX and IMAX.
            if (k < n)
            {
                iMax = k + cblas_isamax(n - k, w + k + (k - 1) * ldw, 1) + 1;
                colMax = fabs(w[iMax - 1 + (k - 1) * ldw]);
            }
            else
            {
                colMax = ZERO;
            }
            if (MAX(absAKK, colMax) == ZERO)
            {
                // Column K is zero or underflow: set INFO and continue
                if (info == 0)
                {
                    info = k;
                }
                kp = k;
                cblas_scopy(n - k + 1, w + k - 1 + (k - 1) * ldw, 1, a + k - 1 + (k - 1) * lda, 1);
                // Set E( K ) to zero
                if (k < n)
                {
                    e[k - 1] = ZERO;
                }
            }
            else
            {
                /*
                Test for interchange
                Equivalent to testing for ABSAKK.GE.ALPHA*COLMAX
                (used to handle NaN and Inf)
                */
                if (absAKK >= ALPHA * colMax)
                {
                    // no interchange, use 1-by-1 pivot block
                    kp = k;
                }
                else
                {
                    flag = 1;
                    // Loop until pivot found
                    while (flag)
                    {
                        // Copy column IMAX to column K+1 of w and update it
                        cblas_scopy(iMax - k, a + iMax - 1 + (k - 1) * lda, lda, w + k - 1 + k * ldw, 1);
                        cblas_scopy(n - iMax + 1, a + iMax - 1 + (iMax - 1) * lda, 1, w + iMax - 1 + k * ldw, 1);
                        if (k > 1)
                        {
                            cblas_sgemv(CblasColMajor, CblasNoTrans, n - k + 1, k - 1, -ONE, a + k - 1, lda,
                                        w + iMax - 1, ldw, ONE, w + k - 1 + k * ldw, 1);
                        }
                        /*
                        JMAX is the column-index of the largest off-diagonal
                        element in row IMAX, and ROWMAX is its absolute value.
                        */
                        if (iMax != k)
                        {
                            jMax = k - 1 + cblas_isamax(iMax - k, w + k - 1 + k * ldw, 1) + 1;
                            rowMax = fabs(w[jMax - 1 + k * ldw]);
                        }
                        else
                        {
                            rowMax = ZERO;
                        }

                        if (iMax < n)
                        {
                            iTemp = iMax + cblas_isamax(n - iMax, w + iMax + k * ldw, 1) + 1;
                            sTemp = fabs(w[iTemp - 1 + k * ldw]);
                            if (sTemp > rowMax)
                            {
                                rowMax = sTemp;
                                jMax = iTemp;
                            }
                        }
                        /*
                        Equivalent to testing for ABS( w( IMAX, K+1 ) ).GE.ALPHA*ROWMAX
                        (used to handle NaN and Inf)
                        */
                        float temp11 = fabs(w[iMax - 1 + k * ldw]);
                        float temp22 = ALPHA * rowMax;
                        if (fabs(w[iMax - 1 + k * ldw]) >= ALPHA * rowMax)
                        {
                            // interchange rows and columns K and IMAX,use 1-by-1 pivot block
                            kp = iMax;
                            // copy column K+1 of w to column K of w
                            cblas_scopy(n - k + 1, w + k - 1 + k * ldw, 1, w + k - 1 + (k - 1) * ldw, 1);
                            flag = 0;
                        }
                        /*
                        Equivalent to testing for ROWMAX.EQ.COLMAX,
                        (used to handle NaN and Inf)
                        */
                        else if ((p == jMax) || (rowMax <= colMax))
                        {
                            // interchange rows and columns K+1 and IMAX,use 2-by-2 pivot block
                            kp = iMax;
                            kStep = 2;
                            flag = 0;
                        }
                        else
                        {
                            // Pivot not found: set params and repeat
                            p = iMax;
                            colMax = rowMax;
                            iMax = jMax;
                            // Copy updated JMAXth (next IMAXth) column to Kth of w
                            cblas_scopy(n - k + 1, w + k - 1 + k * ldw, 1, w + k - 1 + (k - 1) * ldw, 1);
                        }
                    }
                }
                kk = k + kStep - 1;
                if ((kStep == 2) && (p != k))
                {
                    // Copy non-updated column K to column P
                    cblas_scopy(p - k, a + k - 1 + (k - 1) * lda, 1, a + p - 1 + (k - 1) * lda, lda);
                    cblas_scopy(n - p + 1, a + p - 1 + (k - 1) * lda, 1, a + p - 1 + (p - 1) * lda, 1);
                    // Interchange rows K and P in first K columns of a and first K + 1 columns of w
                    cblas_sswap(k, a + k - 1, lda, a + p - 1, lda);
                    cblas_sswap(kk, w + k - 1, ldw, w + p - 1, ldw);
                }
                // Updated column kp is already stored in column KK of w
                if (kp != kk)
                {
                    // Copy non-updated column KK to column KP
                    a[kp - 1 + (k - 1) * lda] = a[kk - 1 + (k - 1) * lda];
                    cblas_scopy(kp - k - 1, a + k + (kk - 1) * lda, 1, a + kp - 1 + k * lda, lda);
                    cblas_scopy(n - kp + 1, a + kp - 1 + (kk - 1) * lda, 1, a + kp - 1 + (kp - 1) * lda, 1);
                    // Interchange rows KK and KP in first KK columns of A and W
                    cblas_sswap(kk, a + kk - 1, lda, a + kp - 1, lda);
                    cblas_sswap(kk, w + kk - 1, ldw, w + kp - 1, ldw);
                }
                if (kStep == 1)
                {
                    /*
                    1-by-1 pivot block D(k): column k of w now holds
                    w(k) = L(k)*D(k)
                    where L(k) is the k-th column of L
                    Store L(k) in column k of a
                    */
                    cblas_scopy(n - k + 1, w + k - 1 + (k - 1) * ldw, 1, a + k - 1 + (k - 1) * lda, 1);
                    if (k < n)
                    {
                        if (fabs(a[k - 1 + (k - 1) * lda]) >= sfMin)
                        {
                            r1 = ONE / a[k - 1 + (k - 1) * lda];
                            cblas_sscal(n - k, r1, a + k + (k - 1) * lda, 1);
                        }
                        else if (a[k - 1 + (k - 1) * lda] != ZERO)
                        {
                            for (ii = k + 1; ii <= n; ii++)
                            {
                                a[ii - 1 + (k - 1) * lda] = a[ii - 1 + (k - 1) * lda] / a[k - 1 + (k - 1) * lda];
                            }
                        }
                        e[k - 1] = ZERO;
                    }
                }
                else
                {
                    /*
                    2-by-2 pivot block D(k): columns k and k+1 of w now hold
                    ( w(k) w(k+1) ) = ( L(k) L(k+1) )*D(k)
                    where L(k) and L(k+1) are the k-th and (k+1)-th columns of L
                    */
                    if (k < n - 1)
                    {
                        // Store L(k) and L(k+1) in columns k and k+1 of a
                        d21 = w[k + (k - 1) * ldw];
                        d11 = w[k + k * ldw] / d21;
                        d22 = w[k - 1 + (k - 1) * ldw] / d21;
                        t = ONE / (d11 * d22 - ONE);
                        for (j = k + 2; j <= n; j++)
                        {
                            a[j - 1 + (k - 1) * lda] = t * ((d11 * w[j - 1 + (k - 1) * ldw] - w[j - 1 + k * ldw]) / d21);
                            a[j - 1 + k * lda] = t * ((d22 * w[j - 1 + k * ldw] - w[j - 1 + (k - 1) * ldw]) / d21);
                        }
                    }
                    /*
                    Copy diagonal elements of D(K) to a,
                    copy subdiagonal element of D(K) to E(K) and
                    ZERO out subdiagonal entry of a
                    */
                    a[k - 1 + (k - 1) * lda] = w[k - 1 + (k - 1) * ldw];
                    a[k + (k - 1) * lda] = ZERO;
                    a[k + k * lda] = w[k + k * ldw];
                    e[k - 1] = w[k + (k - 1) * ldw];
                    e[k] = ZERO;
                }
                // End column K is nonsingular
            }
            // Store details of the interchanges in IPIV
            if (kStep == 1)
            {
                ipiv[k - 1] = kp;
            }
            else
            {
                ipiv[k - 1] = -p;
                ipiv[k] = -kp;
            }
            // Increase K and return to the start of the main loop
            k += kStep;
        }
        /*
        Update the lower triangle of A22 (= a(k:n,k:n)) as
        A22 := A22 - L21*D*L21**T = A22 - L21*w**T
        computing blocks of NB columns at a time
        */
        for (j = k; j <= n; j += nb)
        {
            jb = MIN(nb, n - j + 1);
            // Update the lower triangle of the diagonal block
            for (jj = j; jj <= j + jb - 1; jj++)
            {
                cblas_sgemv(CblasColMajor, CblasNoTrans, j + jb - jj, k - 1, -ONE, a + jj - 1,
                            lda, w + jj - 1, ldw, ONE, a + jj - 1 + (jj - 1) * lda, 1);
            }
            // Update the rectangular subdiagonal block
            if (j + jb <= n)
            {
                cblas_sgemm(CblasColMajor, CblasNoTrans, CblasTrans, n - j - jb + 1, jb, k - 1, -ONE, a + j + jb - 1,
                            lda, w + j - 1, ldw, ONE, a + j + jb - 1 + (j - 1) * lda, lda);
            }
        }
        // Set KB to the number of columns factorized
        kb = k - 1;
    }
    return;
}
